//
// Created by clarence on 2021/5/13.
//

#include <ros/ros.h>

#include <tf/transform_datatypes.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <algorithm>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <stdlib.h>
#include <mavros_msgs/State.h>
#include <random>
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <vector>
#include <fstream>
#include <cstdint>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include "vied_map_array_improve_5_cluster_velocity_zyx.h"
#include "gazebo_msgs/ModelStates.h"
#include "gazebo_msgs/ModelState.h"

using namespace message_filters;
using namespace std;

#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708


/**** Parameters to tune, some initialization needs to be changed in main function ****/
VideMap my_map;

const unsigned int MAX_POINT_NUM  = 5000;
float point_clouds[MAX_POINT_NUM*4];

float x_min = -MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float x_max = MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float y_min = -MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float y_max = MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float z_min = -MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float z_max = MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float res = 0.15;  // Smaller res will get better tracking result but is slow.

float future_status[VOXEL_NUM][PREDICTION_TIMES];
float future_status_copy[VOXEL_NUM][PREDICTION_TIMES];

Eigen::Vector3d map_center_point;

Eigen::Vector3d last_send_p, last_send_v, last_send_a;

const float CAL_DURATION = 0.025f; // 40Hz

const int ANGLE_H_NUM_SPARSE = 23;
const int ANGLE_V_NUM_SPARSE = 7;

float MAX_V_XY = 1.f;
float MAX_V_Z_UP = 1.f;
float MAX_V_Z_DOWN = 0.5f;
float MAX_A = 1.5f;
float HEIGHT_LIMIT = 2.f;
float XY_LIMIT = 2.5f;

float ABSOLUTE_SAFE_TIME_PIECE1 = 1.0f;
float ABSOLUTE_SAFE_TIME_PIECE2 = 1.0f;

float MAX_RISK_ONE_PRIMITIVE = 1000.0;

float DIRECTION_CHANGE_LEFT_SIZE = 1.5;

float PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;

bool DIRECTION_AUTO_CHANGE = false;

bool map_status_locked = false;
bool state_locked = false;

struct  Path_Planning_Parameters
{
    float d_ref = 1.6;  //NOTE: NEED TO CHANGE CHG
    float k1_xy = 5; //% Goal directed coefficient
    float k1_z = 3; //% Goal directed coefficient
    float k2_xy = 1; //% Rotation coefficient 3
    float k2_z = 1.5; //% Rotation coefficient 3.5
    float v_max_at_goal_ori = 0.3; //% m/s, just reference  5.0 originally
    float v_scale_min = 0.1;
    float collision_threshold_static = 0.6;
    float dynamic_future_risk_threshold = 5.0;
    float risk_checking_radius = 1.2;
    int max_plan_num = ANGLE_H_NUM_SPARSE * ANGLE_V_NUM_SPARSE;  // Previously it was 100, as 18*7 = 126 > 100
}pp;


float MAP_DELAY_SECONDS = 0.05;
const int point_num_pub = 5; // For the visualization of current planned trajectory

bool safe_trajectory_avaliable = true;
bool use_position_global_time = false;
bool if_plan_vertical_path = true;
bool if_flyback_enable = true;
bool if_in_simulation = true;
bool trajectory_initialized = false;


std::queue<double> pose_timestamp_queue;
std::queue<Eigen::Quaternionf> att_queue;
std::queue<Eigen::Vector3d> pos_queue;

ros::Publisher cloud_pub, future_status_pub, map_center_pub;


/****** Global variables for path planning ******/
Eigen::VectorXf Fov_half(2); //Fov parameters
Eigen::VectorXf Angle_h_sparse(ANGLE_H_NUM_SPARSE);  // initiate later in the main function, rad
Eigen::VectorXf Angle_v_sparse(ANGLE_V_NUM_SPARSE); // initiate later in the main function, rad

Eigen::Vector3d p_goal;
Eigen::Vector3d p_goal_raw;
Eigen::Vector3d p0;
Eigen::Vector3d v0;
Eigen::Vector3d a0;
Eigen::Quaternionf quad(1.0, 0.0, 0.0, 0.0);
Eigen::Vector3d init_p;
float yaw0;
float yaw0_rate;

float theta_h_last = 0.0;
float theta_v_last = 0.0;

vector<Eigen::Vector3d> traj_p_buffer_1, traj_p_buffer_2;
vector<Eigen::Vector3d> traj_v_buffer_1, traj_v_buffer_2;
vector<Eigen::Vector3d> traj_a_buffer_1, traj_a_buffer_2;

Eigen::MatrixXd traj_p_buffer_2_matrix;

bool trajectory_buffer_1_requires_update = true;
bool trajectory_buffer_2_requires_update = true;
int trajectory_1_send_counter = 0;

struct ActorState{
    string name;
    double record_time;
    double px = 0.f;
    double py = 0.f;
    double pz = 0.f;
    double yaw = 0.f;
    double vx = 0.f;
    double vy = 0.f;
    double vz = 0.f;
};

vector<ActorState> actor_states;
vector<ActorState> actor_avg_states;

ros::Publisher pva_pub, current_marker_pub;

void setParameters();

void marker_publish(const Eigen::MatrixXd &Points, int id, float r, float g, float b, float width, int publish_num);

void marker_publish(const vector<Eigen::Vector3d> &Points, int id, float r, float g, float b, float width, int publish_num);

/**********************FOR HSH TABLE***************************/
// PVA table for feasible total planning time T
typedef struct pva_table pva_table;
struct pva_table {
    int dim_num;
    int *dim_size;
    uint64_t *dim_interval;
    uint64_t table_size;
    float *table;
    float rez;
    float *pva_limit;

    void construct_pva_table(int dim1_size, int dim2_size, int dim3_size, int dim4_size, float resolution) {
        this->dim_num = 4;
        this->dim_size = (int*)malloc(sizeof(int)*this->dim_num);
        this->dim_interval = (uint64_t*)malloc(sizeof(uint64_t)*this->dim_num);

        this->dim_size[0] = dim1_size;
        this->dim_size[1] = dim2_size;
        this->dim_size[2] = dim3_size;
        this->dim_size[3] = dim4_size;

        this->dim_interval[3] = 1;
        this->dim_interval[2] = dim_interval[3] * dim4_size;
        this->dim_interval[1] = dim_interval[2] * dim3_size;
        this->dim_interval[0] = dim_interval[1] * dim2_size;

        this->table_size = this->dim_interval[0] * dim1_size;
        this->table = (float*)malloc(sizeof(float)*this->table_size);

        this->rez = resolution;

        this->pva_limit = (float*)malloc(sizeof(float)*3);
        this->pva_limit[0] = this->rez*float(dim1_size/2);
        this->pva_limit[1] = this->rez*float(dim2_size/2);
        this->pva_limit[2] = this->rez*float(dim4_size/2);
    }

    void compute_idx_from_pva(float dlt_p, float v0, float vf, float a0,
                              int &idx1, int &idx2, int &idx3, int &idx4) {
        idx1 = round(dlt_p/this->rez) + this->dim_size[0]/2;
        idx2 = round(v0/this->rez) + this->dim_size[1]/2;
        idx3 = round(vf/this->rez) + this->dim_size[2]/2;
        idx4 = round(a0/this->rez) + this->dim_size[3]/2;
    }

    float query_pva_table(float dlt_p, float v0, float vf, float a0) {
        if (fabs(dlt_p) > this->pva_limit[0]) static_assert("Invalid input!", "");
        if (fabs(v0) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(vf) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(a0) > this->pva_limit[2]) static_assert("Invalid input!", "");

        int idx1, idx2, idx3, idx4;
        this->compute_idx_from_pva(dlt_p, v0, vf, a0, idx1, idx2, idx3, idx4);

        uint64_t idx = idx1*this->dim_interval[0] + idx2*this->dim_interval[1] +
                       idx3*this->dim_interval[2] + idx4*this->dim_interval[3];

        // std::cout << "idx: " << idx << std::endl;

        return this->table[idx];
    }

    void pva_table2csv(const std::string &str) {
        std::ofstream outfile;
        outfile.open(str, std::ios::out);

        for (int i = 0; i < 4; ++i) outfile << std::to_string(this->dim_size[i]) << ',';
        outfile << std::to_string(this->rez) << std::endl;

        for (uint64_t i = 0; i < this->table_size-1; ++i) outfile << std::to_string(this->table[i]) << ',';
        outfile << std::to_string(this->table[table_size-1]);

        outfile.close();
    }

    void csv2pva_table(const std::string &str) {
        int tmp_dim_size[4];
        float tmp_rez;

        std::ifstream infile(str, std::ios::in);
        std::string tmp_str;

        for (int i = 0; i < 4; ++i) {
            getline(infile, tmp_str, ',');
            tmp_dim_size[i] = std::stoi(tmp_str);
        }

        getline(infile, tmp_str);
        tmp_rez = std::stod(tmp_str);

        this->construct_pva_table(tmp_dim_size[0], tmp_dim_size[1],
                                  tmp_dim_size[2], tmp_dim_size[3], tmp_rez);

        for (uint64_t i = 0; i < this->table_size; ++i) {
            getline(infile, tmp_str, ',');
            this->table[i] = std::stod(tmp_str);
        }
    }

    void free_pva_table() {
        free(this->pva_limit);
        free(this->table);
        free(this->dim_interval);
        free(this->dim_size);
    }
};

pva_table *trajectory_time_table = (pva_table*)malloc(sizeof(pva_table));
/***************************************************/

/** This function is to generate state to state trajectory **/
bool motion_primitives_with_table(Eigen::Vector3d p0, Eigen::Vector3d v0, Eigen::Vector3d a0, float yaw0, float theta_h,
                                  float theta_v, Eigen::Vector3d goal, float d, float v_final, float delt_t,
                                  Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    float delt_x = d*cos(theta_v)*cos(theta_h+yaw0);
    float delt_y = d*cos(theta_v)*sin(theta_h+yaw0);
    float delt_z = d*sin(theta_v);

    Eigen::Vector3d pf;
    pf << p0(0)+delt_x, p0(1)+delt_y, p0(2)+delt_z;

    Eigen::Vector3d l = goal - pf;
    Eigen::Vector3d vf = (v_final / l.norm()) * l;
    vf(2) = 0; // % Note: 0 maybe better, for the p curve wont go down to meet the vf

    Eigen::Vector3d af = Eigen::Vector3d::Zero();

    float j_limit = 4;
    float a_limit = MAX_A;
    float v_limit = MAX_V_XY;

    for(int i=0; i<3; i++){
        if(fabs(v0(i)) > MAX_V_XY) {
            v0(i) = MAX_V_XY * v0(i) / fabs(v0(i));
            ROS_WARN_THROTTLE(1, "v_limit in local planning is large than the values in the loaded table!  v0 max=%f", v0.cwiseAbs().maxCoeff());
        }
        if(fabs(a0(i)) > MAX_A) {
            a0(i) = MAX_A * a0(i) / fabs(a0(i));
            ROS_WARN_THROTTLE(1, "a_limit in local planning is large than the values in the loaded table!  a0 max=%f", a0.cwiseAbs().maxCoeff());
        }
    }

    float T1, T2, T3, T;
    T1 = trajectory_time_table->query_pva_table(delt_x, v0(0), vf(0), a0(0));
    T2 = trajectory_time_table->query_pva_table(delt_y, v0(0), vf(0), a0(0));
    T3 = trajectory_time_table->query_pva_table(delt_z, v0(0), vf(0), a0(0));

    if (T1 == -1 || T2 == -1 || T3 == -1) {
        return false;
    }

    T = T1 > T2 ? T1 : T2;
    T = T > T3 ? T : T3;
//    T *= 1.1;
    T = T < 0.5 ? 0.5 : T;

    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller

    for(int ii=0; ii<3; ii++)
    {
        float delt_a = af(ii) - a0(ii);
        float delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        float delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        // % if vf is not free
        float alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        float beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        float gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            float tt = (jj+1) * delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);
        }
    }

    return true;
}

int getMapCheckSeq(float t){
    static float check_times[PREDICTION_TIMES-1];
    static bool check_initialized = false;
    if(!check_initialized){
        // Set check time
        for(int i=0; i<PREDICTION_TIMES-1; ++i){
            check_times[i] = (prediction_future_time[i] + prediction_future_time[i+1]) / 2.f;
        }
        check_initialized = true;
    }

    int risk_map_seq = PREDICTION_TIMES - 1;
    for(int i=0; i<PREDICTION_TIMES-1; ++i){
        if(t < check_times[i]){
            risk_map_seq = i;
            break;
        }
    }

    return risk_map_seq;
}


float getRiskOfTrajectory(const Eigen::MatrixXd &traj){
    static int check_neighbors_num = ceil(pp.risk_checking_radius / VOXEL_RESOLUTION);


    float risk = 0.f;

    for(int n=0; n<traj.rows(); n+=2){  //check every two points to accelerate
        Eigen::Vector3d p_temp = traj.row(n);
        Eigen::Vector3d p = p_temp - map_center_point;

        if(p.squaredNorm() < 0.64){  //ignore the points that are too close
            continue;
        }

        float t = CAL_DURATION * (float)n;
        int risk_map_seq = getMapCheckSeq(t);

        float risk_this_point = 0.f;
        for(int i = -check_neighbors_num; i <= check_neighbors_num; ++i)        {
            for(int j = -check_neighbors_num; j <= check_neighbors_num; ++j){
                for(int k = -check_neighbors_num; k <= check_neighbors_num; ++k) {
                    Eigen::Vector3d p_check;
                    p_check.x() = p.x() + (double)i*VOXEL_RESOLUTION;
                    p_check.y() = p.y() + (double)j*VOXEL_RESOLUTION;
                    p_check.z() = p.z() + (double)k*VOXEL_RESOLUTION;

                    int index;
                    if(my_map.getPointVoxelsIndexPublic(p_check.x(), p_check.y(), p_check.z(), index))
                    {
                        risk_this_point += future_status_copy[index][risk_map_seq];
                    }

                }
            }
        }

        risk += min(risk_this_point, 5.f);
    }

    // Set a risk limitation
    if(risk > MAX_RISK_ONE_PRIMITIVE){
        risk = MAX_RISK_ONE_PRIMITIVE;
    }

    return risk;
}


bool collisionCheckingWithoutRisk(const vector<Eigen::Vector3d> &traj)
{
    static int check_neighbors_num = ceil(pp.collision_threshold_static / VOXEL_RESOLUTION);

    double last_dist = 0.f;
    double last_height_dist = -1000.f;

    for(int n=0; n<traj.size(); n += 2) {
        Eigen::Vector3d p = traj[n] - map_center_point;

        // height limit
        double height_dist = min(HEIGHT_LIMIT-traj[n].z(), traj[n].z());
        if(height_dist < 0.3f && height_dist < last_height_dist){
            return false;
        }
        last_height_dist = height_dist;

        float t = CAL_DURATION * (float)n;
        int risk_map_seq = getMapCheckSeq(t);

        double dist_min_temp_this = 1000.f;

        for(int i = -check_neighbors_num; i <= check_neighbors_num; ++i)        {
            for(int j = -check_neighbors_num; j <= check_neighbors_num; ++j){
                for(int k = -check_neighbors_num; k <= check_neighbors_num; ++k) {
                    Eigen::Vector3d p_check;
                    double delt_p_x = (double)i*VOXEL_RESOLUTION;
                    double delt_p_y = (double)j*VOXEL_RESOLUTION;
                    double delt_p_z = (double)k*VOXEL_RESOLUTION;
                    p_check.x() = p.x() + delt_p_x;
                    p_check.y() = p.y() + delt_p_y;
                    p_check.z() = p.z() + delt_p_z;

                    int index;
                    if(my_map.getPointVoxelsIndexPublic(p_check.x(), p_check.y(), p_check.z(), index))
                    {
                        if(future_status_copy[index][risk_map_seq] > 0.6){ ///TODO: Use a better threshold
                            auto dist = sqrt(delt_p_x*delt_p_x + delt_p_y*delt_p_y + delt_p_z*delt_p_z);

                            if(dist < dist_min_temp_this){
                                dist_min_temp_this = dist;
                            }
                        }
                    }

                }
            }
        }

        if(dist_min_temp_this < pp.collision_threshold_static){
            if(dist_min_temp_this <= last_dist){ //Not far away from the obstacle
                return false;
            }
        }

        if(dist_min_temp_this > 900.f){
            last_dist = 0.f;
        }else{
            last_dist = dist_min_temp_this;
        }
    }

    return true;
}



bool collisionCheckingAndRiskPredictionDynamic(const Eigen::MatrixXd &traj,  float &risk, double absolute_safe_time)
{

    double collision_checking_start_time = ros::Time::now().toSec();

    auto collisionCheckingPointsNum = (int)(absolute_safe_time / CAL_DURATION);
    vector<Eigen::Vector3d> checkingTraj;

    if(collisionCheckingPointsNum > traj.rows()) {collisionCheckingPointsNum = traj.rows();}

    for(int i=0; i<collisionCheckingPointsNum; ++i){
        checkingTraj.emplace_back(traj.row(i));
    }

    if(!collisionCheckingWithoutRisk(checkingTraj)){
        return false;
    }

    ROS_INFO_THROTTLE(0.5, "collision_checking time is %lf s", ros::Time::now().toSec()-collision_checking_start_time);

    double risk_prediction_start_time = ros::Time::now().toSec();
    risk = getRiskOfTrajectory(traj);
    ROS_INFO_THROTTLE(0.5, "risk = %f", risk);
    if(risk > pp.dynamic_future_risk_threshold){
        return false;
    }

    ROS_INFO_THROTTLE(0.5, "risk_prediction time is %lf s", ros::Time::now().toSec()-risk_prediction_start_time);


    return true;
}


bool trajectoryPlanOnePiece(Eigen::Vector3d p_start, Eigen::Vector3d v_start, Eigen::Vector3d a_start, float yaw_start,
                            Eigen::MatrixXd &traj_p_buffer_chosen, Eigen::MatrixXd &traj_v_buffer_chosen,
                            Eigen::MatrixXd &traj_a_buffer_chosen, int piece_seq = 1, float absolute_safe_time = 1.f){

    static bool first_plan = true;
    static float last_angle_h = 0.f;
    static float last_angle_v = 0.f;
    float angle_h_chosen = 0.f;
    float angle_v_chosen = 0.f;

    vector<Eigen::MatrixXd> primitive_candidate_p;
    vector<Eigen::MatrixXd> primitive_candidate_v;
    vector<Eigen::MatrixXd> primitive_candidate_a;
    vector<float> primitive_candidate_risk;
    vector<float> primitive_candidate_angle_h;
    vector<float> primitive_candidate_angle_v;

    Eigen::Vector3d delt_p = p_goal - p_start;
    float phi_h = atan2(delt_p(1), delt_p(0)); //% horizental offset angle
    float phi_v = atan2(delt_p(2), sqrt(delt_p(0) * delt_p(0) + delt_p(1) * delt_p(1))); //% vertical offset angle


    for(int i=0; i < ANGLE_H_NUM_SPARSE; i++)
    {
        for(int j=0; j < ANGLE_V_NUM_SPARSE; j++)
        {
            float angle_h_this = Angle_h_sparse(i) + phi_h;
            float angle_v_this = Angle_v_sparse(j) + phi_v;

            Eigen::MatrixXd p;
            Eigen::MatrixXd v;
            Eigen::MatrixXd a;
            Eigen::VectorXd t;

            if(motion_primitives_with_table(p_start, v_start, a_start, yaw_start, angle_h_this, angle_v_this, p_goal,
                                            pp.d_ref, pp.v_max_at_goal_ori, CAL_DURATION, p, v, a, t)){

                // Collision checking and risk prediction
                float risk = 0.0;
                if(collisionCheckingAndRiskPredictionDynamic(p,risk, absolute_safe_time)){
                    primitive_candidate_p.push_back(p);
                    primitive_candidate_v.push_back(v);
                    primitive_candidate_a.push_back(a);
                    primitive_candidate_risk.push_back(risk);
                    primitive_candidate_angle_h.push_back(angle_h_this);
                    primitive_candidate_angle_v.push_back(angle_v_this);
                }
            }

        }
    }

    if(primitive_candidate_p.empty()){
        ROS_INFO("No safe path, piece_seq=%d", piece_seq);
        return false;
    }

    // Find the best trajectory
    float cost_min = 100000.f;

    for(int i=0; i<primitive_candidate_p.size(); ++i){
        float cost_goal = pp.k1_xy * (primitive_candidate_angle_h[i] - phi_h) * (primitive_candidate_angle_h[i] - phi_h) +
                          pp.k1_z * (primitive_candidate_angle_v[i] - phi_v) * (primitive_candidate_angle_v[i] - phi_v);

        float cost_direction_change = 0.f;
        if(!first_plan && piece_seq == 1){  // only for the first piece
            float delt_angle_h = primitive_candidate_angle_h[i]  - last_angle_h;
            float delt_angle_v = primitive_candidate_angle_v[i]  - last_angle_v;
            cost_direction_change = pp.k2_xy * delt_angle_h*delt_angle_h + pp.k2_z * delt_angle_v*delt_angle_v;
        }

        float total_cost = cost_goal + cost_direction_change + primitive_candidate_risk[i];
        if(total_cost < cost_min){
            cost_min = total_cost;
            traj_p_buffer_chosen = primitive_candidate_p[i];
            traj_v_buffer_chosen = primitive_candidate_v[i];
            traj_a_buffer_chosen = primitive_candidate_a[i];
        }
    }

    if(piece_seq == 1){  // only for the first piece
        first_plan = false;
        last_angle_h = angle_h_chosen;
        last_angle_v = angle_v_chosen;
    }

    return true;
}

void trajectoryCallback(const ros::TimerEvent& e) {
//    static Eigen::MatrixXd whole_traj1_to_visualize, whole_traj2_to_visualize;

    double planning_start_time = ros::Time::now().toSec();

    static const int points_num_one_piece1 = (int)(ABSOLUTE_SAFE_TIME_PIECE1 / CAL_DURATION);
    static const int points_num_one_piece2 = (int)(ABSOLUTE_SAFE_TIME_PIECE2 / CAL_DURATION);

    Eigen::Vector3d current_p = p0;
    Eigen::Vector3d current_v = v0;
    Eigen::Vector3d current_a = a0;
    float current_yaw = yaw0;

    if(!trajectory_initialized){
        last_send_p = current_p;
        last_send_v << 0.0, 0.0, 0.0;
        last_send_a << 0.0, 0.0, 0.0;
    }


    if(!map_status_locked){
        map_status_locked = true;
        for(int i=0; i<VOXEL_NUM; ++i){
            for(int j=0; j<PREDICTION_TIMES; ++j){
                future_status_copy[i][j] = future_status[i][j];
            }
        }
        map_status_locked = false;
    }



    /// Left points piece one
    vector<Eigen::Vector3d> left_points_piece_one;
    for(int i=trajectory_1_send_counter; i<traj_p_buffer_1.size(); ++i){
        left_points_piece_one.emplace_back(traj_p_buffer_1[i]);
    }
    // Visualization
    marker_publish(left_points_piece_one, 3, 0.f, 1.f, 0.f, 0.1, 8);


    /// Check if need to replace trajectory 1 with 2.
    static const int transfer_buffer_size = 5;
    if(trajectory_initialized && trajectory_1_send_counter >= traj_p_buffer_1.size() - transfer_buffer_size){ /// frequency and calculation frequency usually don't match. Use buffer with transfer_buffer_size points.

        if(!traj_p_buffer_2.empty()){
            vector<Eigen::Vector3d> transfer_buffer_p_temp;
            vector<Eigen::Vector3d> transfer_buffer_v_temp;
            vector<Eigen::Vector3d> transfer_buffer_a_temp;
            for(int j= traj_p_buffer_1.size() - trajectory_1_send_counter - 1; j>0; --j){
                transfer_buffer_p_temp.push_back(traj_p_buffer_1[traj_p_buffer_1.size() - j]);
                transfer_buffer_v_temp.push_back(traj_v_buffer_1[traj_v_buffer_1.size() - j]);
                transfer_buffer_a_temp.push_back(traj_a_buffer_1[traj_a_buffer_1.size() - j]);
            }

            traj_p_buffer_1 = transfer_buffer_p_temp;
            traj_v_buffer_1 = transfer_buffer_v_temp;
            traj_a_buffer_1 = transfer_buffer_a_temp;
            trajectory_1_send_counter = 0;

            int new_start_seq = traj_p_buffer_1.size();

            for(int i=0; i<traj_p_buffer_2.size(); ++i){  // 0 is ignore because it is the same point as the last point in original traj1
                traj_p_buffer_1.push_back(traj_p_buffer_2[i]);
                traj_v_buffer_1.push_back(traj_v_buffer_2[i]);
                traj_a_buffer_1.push_back(traj_a_buffer_2[i]);
            }

            trajectory_buffer_2_requires_update = true;

            ROS_INFO("Replaced Traj1 with Traj2, tracked_point=(%f, %f, %f), new_point=(%f, %f, %f), tracked_vel=(%f, %f, %f), new_vel=(%f, %f, %f)",
                     last_send_p(0),last_send_p(1),last_send_p(2), traj_p_buffer_1[0](0), traj_p_buffer_1[0](1), traj_p_buffer_1[0](2),
                     last_send_v(0), last_send_v(1), last_send_v(2), traj_v_buffer_1[0](0), traj_v_buffer_1[0](1), traj_v_buffer_1[0](2));

        }else{
            ROS_INFO("No valid Traj2 to replace, will replan traj1");
            trajectory_buffer_1_requires_update = true;
            trajectory_buffer_2_requires_update = true;
        }

    }

    /// Check if replanning is necessary
    static float replanning_tracking_error_threshold = 0.6f;
    static float start_point_tracking_error_threshold = 0.3f;

    if(!trajectory_buffer_1_requires_update){
        Eigen::Vector3d tracking_p_error = current_p - last_send_p;
        if(tracking_p_error.norm() > 5.f){
            ROS_ERROR("current_p=(%f,%f), sp=(%f,%f)", current_p(0), current_p(1), last_send_p(0), last_send_p(1));
        }

        if(tracking_p_error.norm() > replanning_tracking_error_threshold){
            trajectory_buffer_1_requires_update = true;
            trajectory_buffer_2_requires_update = true;
            ROS_WARN_THROTTLE(0.1, "Replan traj1 because of tracking error. current_p=(%f,%f), sp=(%f,%f)", current_p.x(), current_p.y(), traj_p_buffer_1[trajectory_1_send_counter-1].x(), traj_p_buffer_1[trajectory_1_send_counter-1].y());

        }else{
            //check collision, if collision, replan
            if(!collisionCheckingWithoutRisk(left_points_piece_one)){
                trajectory_buffer_1_requires_update = true;
                trajectory_buffer_2_requires_update = true;
                ROS_INFO_THROTTLE(0.3, "Replan traj1 because of collision.");

            }

        }
    }

//    if(!trajectory_buffer_2_requires_update){
//        float piece2_risk = 0.f;
//        bool piece2_free_flag = collisionCheckingAndRiskPredictionDynamic(traj_p_buffer_2_matrix, piece2_risk, ABSOLUTE_SAFE_TIME_PIECE2);
//
//        if(!piece2_free_flag){
//            trajectory_buffer_2_requires_update = true;
//            ROS_INFO_THROTTLE(0.3, "Replan traj2 because of risk.");
//        }
//    }

    trajectory_buffer_2_requires_update = true;

    static int traj2_plan_failed_times = 0;
    if(traj2_plan_failed_times > 3){
        trajectory_buffer_1_requires_update = true;
        traj2_plan_failed_times = 0;
    }

    /// Update piece 1
    if(trajectory_buffer_1_requires_update){
        Eigen::MatrixXd traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen;

        Eigen::Vector3d start_p, start_v, start_a;
        if(trajectory_initialized){
            Eigen::Vector3d tracking_p_error = current_p - last_send_p;
            if(tracking_p_error.norm() > start_point_tracking_error_threshold){
                start_p = current_p * 0.7 + last_send_p * 0.3;
                start_v = current_v * 0.7 + last_send_v * 0.3;
                start_a = current_a * 0.7 + last_send_a * 0.3;
            }else{
                start_p = last_send_p;
                start_v = last_send_v;
                start_a = last_send_a;
            }
        }else{
            start_p = current_p;
            start_v = current_v;
            start_a = current_a;
        }

        ROS_INFO_THROTTLE(0.5, "Start v =(%f, %f, %f)", start_v(0), start_v(1), start_v(2));

        traj_p_buffer_1.clear();
        traj_v_buffer_1.clear();
        traj_a_buffer_1.clear();

        bool plan_successful_flag = trajectoryPlanOnePiece(start_p, start_v, start_a, current_yaw, traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen, 1, ABSOLUTE_SAFE_TIME_PIECE1);

        if(plan_successful_flag){
            int point_num = points_num_one_piece1;
            if(point_num > traj_p_buffer_chosen.rows()){point_num = traj_p_buffer_chosen.rows();}

            for(int i=0; i<point_num; ++i){
                traj_p_buffer_1.emplace_back(traj_p_buffer_chosen.row(i));
                traj_v_buffer_1.emplace_back(traj_v_buffer_chosen.row(i));
                traj_a_buffer_1.emplace_back(traj_a_buffer_chosen.row(i));
                trajectory_1_send_counter = 0;
            }

//            whole_traj1_to_visualize = traj_p_buffer_chosen;

            trajectory_buffer_1_requires_update = false;
            safe_trajectory_avaliable = true;
            trajectory_initialized = true;
        }
        else{
            /// TODO: Should enter safety mode
            safe_trajectory_avaliable = false;
            return;
        }

    }

    /// Update piece 2
    if(trajectory_buffer_2_requires_update && safe_trajectory_avaliable){
        Eigen::MatrixXd traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen;

        traj_p_buffer_2.clear();
        traj_v_buffer_2.clear();
        traj_a_buffer_2.clear();

        bool plan_successful_flag = trajectoryPlanOnePiece(traj_p_buffer_1[traj_p_buffer_1.size()-1], traj_v_buffer_1[traj_v_buffer_1.size()-1],
                                                           traj_a_buffer_1[traj_a_buffer_1.size()-1], yaw0, traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen, 2, ABSOLUTE_SAFE_TIME_PIECE2);
        if(plan_successful_flag){
            int point_num = points_num_one_piece2;
            if(point_num > traj_p_buffer_chosen.rows()){point_num = traj_p_buffer_chosen.rows();}

            for(int i=0; i<point_num; ++i){
                traj_p_buffer_2.emplace_back(traj_p_buffer_chosen.row(i));
                traj_v_buffer_2.emplace_back(traj_v_buffer_chosen.row(i));
                traj_a_buffer_2.emplace_back(traj_a_buffer_chosen.row(i));
            }

            trajectory_buffer_2_requires_update = false;
            traj_p_buffer_2_matrix = traj_p_buffer_chosen;
            traj2_plan_failed_times = 0;

//            whole_traj2_to_visualize = traj_p_buffer_chosen;
        }else{
            ++ traj2_plan_failed_times;
        }

    }


    marker_publish(traj_p_buffer_1, 1, 0.8f, 0.8f, 0.8f, 0.05, 8);
    marker_publish(traj_p_buffer_2, 2, 0.2f, 0.6f, 0.7f, 0.1, 10);

//    if(whole_traj1_to_visualize.rows() > 0) marker_publish(whole_traj1_to_visualize, 4, 0.6f, 0.5f, 0.1f, 0.06, 10);
//    if(whole_traj2_to_visualize.rows() > 0) marker_publish(whole_traj2_to_visualize, 5, 0.8f, 0.8f, 0.8f, 0.06, 10);

    double planning_end_time = ros::Time::now().toSec();
    ROS_INFO_THROTTLE(0.5, "Planning time is %lf s", planning_end_time-planning_start_time);
}


void sendCallback(const ros::TimerEvent& e)
{
    if(!trajectory_initialized){
        return;
    }

    static Eigen::Vector3d p_store_for_em;

    static bool init_flag = true;
    if(init_flag){
        p_store_for_em << p0(0), p0(1), p0(2);
        init_flag = false;
    }

    static float def_ori = pp.d_ref;

    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    float z_p_set, z_v_set, z_a_set;
    if(if_plan_vertical_path){
        z_p_set = traj_p_buffer_1[trajectory_1_send_counter](2);
        z_v_set = traj_v_buffer_1[trajectory_1_send_counter](2);
        z_a_set = traj_a_buffer_1[trajectory_1_send_counter](2);
    }
    else{
        z_p_set = p_goal(2);
        z_v_set = 0.f;
        z_a_set = 0.f;
    }

    if(safe_trajectory_avaliable){
        pp.d_ref = def_ori;

        pva_setpoint.positions.push_back(traj_p_buffer_1[trajectory_1_send_counter](0)); //x
        pva_setpoint.positions.push_back(traj_p_buffer_1[trajectory_1_send_counter](1)); //y
        pva_setpoint.positions.push_back(z_p_set); //z
        pva_setpoint.positions.push_back(0);  //yaw

        pva_setpoint.velocities.push_back(traj_v_buffer_1[trajectory_1_send_counter](0));
        pva_setpoint.velocities.push_back(traj_v_buffer_1[trajectory_1_send_counter](1));
        pva_setpoint.velocities.push_back(z_v_set);

        pva_setpoint.accelerations.push_back(traj_a_buffer_1[trajectory_1_send_counter](0));
        pva_setpoint.accelerations.push_back(traj_a_buffer_1[trajectory_1_send_counter](1));
        pva_setpoint.accelerations.push_back(z_a_set);

        p_store_for_em << p0(0), p0(1), p0(2);

        ++ trajectory_1_send_counter;
        if(trajectory_1_send_counter >= traj_a_buffer_1.size()){
            -- trajectory_1_send_counter;
            safe_trajectory_avaliable = false;
            ROS_WARN_THROTTLE(0.1, "No points in the buffer.");
        }

        last_send_p = traj_p_buffer_1[trajectory_1_send_counter];
        last_send_v = traj_v_buffer_1[trajectory_1_send_counter];
        last_send_a = traj_a_buffer_1[trajectory_1_send_counter];

    }else{  //emergency stop
        pp.d_ref = 0.5;

        pva_setpoint.positions.push_back(p_store_for_em(0)); //x
        pva_setpoint.positions.push_back(p_store_for_em(1)); //y
        pva_setpoint.positions.push_back(z_p_set); //z
        pva_setpoint.positions.push_back(0);  //yaw

        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);

        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);

        ROS_WARN_THROTTLE(0.3, "Safety mode.");

        last_send_p = p_store_for_em;
        last_send_v << 0.0, 0.0, 0.0;
        last_send_a << 0.0, 0.0, 0.0;
    }

    pva_pub.publish(pva_setpoint);
}


int inRange(float &low, float &high, float &x)
{
    if(x > low && x < high){
        return 1;
    }else{
        return 0;
    }
}

void colorAssign(int &r, int &g, int &b, float v, float value_min=0.f, float value_max=1.f, int reverse_color=0)
{
    v = std::max(v, value_min);
    v = std::min(v, value_max);

    float v_range = value_max - value_min;
    int value = floor((v - value_min) / v_range * 240); // Mapping 0~1.0 to 0~240
    value = std::min(value, 240);

    if(reverse_color){
        value = 240 - value;
    }

    int section = value / 60;
    float float_key = (value % 60) / (float)60 * 255;
    int key = floor(float_key);
    int nkey = 255 - key;

    switch(section) {
        case 0: // G increase
            r = 255;
            g = key;
            b = 0;
            break;
        case 1: // R decrease
            r = nkey;
            g = 255;
            b = 0;
            break;
        case 2: // B increase
            r = 0;
            g = 255;
            b = key;
            break;
        case 3: // G decrease
            r = 0;
            g = nkey;
            b = 255;
            break;
        case 4:
            r = 0;
            g = 0;
            b = 255;
            break;
        default: // White
            r = 255;
            g = 255;
            b = 255;
    }

}



pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
    /*** For cloud ***/
    Eigen::Quaternionf quad_sychronized = quad;  //initialze in case no point in the queue satisfy
    Eigen::Vector3d current_position_sychronized = p0;

    static Eigen::Quaternionf quad_last_popped(-10.f, -10.f, -10.f, -10.f);
    static Eigen::Vector3d position_last_popped(-10000.f, -10000.f, -10000.f);
    static double last_popped_time = 0.0;

    ros::Rate loop_rate(500);
    while(state_locked){
        loop_rate.sleep();
        ros::spinOnce();
    }
    state_locked = true;

    while(!pose_timestamp_queue.empty()){   //Synchronize pose by queue
        double time_stamp_pose = pose_timestamp_queue.front();
        if(time_stamp_pose >= cloud->header.stamp.toSec()){
            quad_sychronized = att_queue.front();
            current_position_sychronized = pos_queue.front();

            // linear interpolation
            if(quad_last_popped.x() >= -1.f){
                double time_interval_from_last_time = time_stamp_pose - last_popped_time;
                double time_interval_cloud = cloud->header.stamp.toSec() - last_popped_time;
                double factor = time_interval_cloud / time_interval_from_last_time;
                quad_sychronized = quad_last_popped.slerp(factor, quad_sychronized);
                current_position_sychronized = position_last_popped * (1.0 - factor) + current_position_sychronized*factor;
            }

            ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf", cloud->header.stamp.toSec() - time_stamp_pose);

            break;
        }

        quad_last_popped = att_queue.front();
        position_last_popped = pos_queue.front();
        last_popped_time = time_stamp_pose;

        pose_timestamp_queue.pop();
        att_queue.pop();
        pos_queue.pop();
    }
    state_locked = false;

    // Add the rotation of head
    float motor_yaw = 0.f;  ///TODO: Update motor yaw
    Eigen::Quaternionf axis_motor;
    axis_motor.w() = cos(motor_yaw/2.0);
    axis_motor.x() = 0;
    axis_motor.y() = 0;
    axis_motor.z() = sin(motor_yaw/2.0);
    Eigen::Quaternionf quad_rotate = quad_sychronized * axis_motor;


    /// Point cloud process
    static double last_time = ros::Time::now().toSec();
    double this_time = ros::Time::now().toSec();

    // convert cloud to pcl form
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*cloud, *cloud_in);

    // down-sample for all
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_in);
    sor.setLeafSize(res, res, res);
    sor.filter(*cloud_filtered);

    int useful_point_num = 0;
    for(int i=0; i<cloud_filtered->width; i++){
        float x = cloud_filtered->points.at(i).z;
        float y = -cloud_filtered->points.at(i).x;
        float z = -cloud_filtered->points.at(i).y;

        if(inRange(x_min, x_max, x) && inRange(y_min, y_max, y) && inRange(z_min, z_max, z))
        {
            point_clouds[useful_point_num*3] = x;
            point_clouds[useful_point_num*3+1] = y;
            point_clouds[useful_point_num*3+2] = z;
            ++ useful_point_num;

            if(useful_point_num >= MAX_POINT_NUM){
                break;
            }
        }
    }


    /// Update
    clock_t start1, finish1;
    start1 = clock();

    my_map.update(useful_point_num, 3, point_clouds,
                  current_position_sychronized.x(), current_position_sychronized.y(), current_position_sychronized.z(), this_time,
                  quad_rotate.w(), quad_rotate.x(), quad_rotate.y(), quad_rotate.z());

    map_center_point = current_position_sychronized;

    finish1 = clock();
    float duration1 = (float)(finish1 - start1) / CLOCKS_PER_SEC;
    ROS_INFO_THROTTLE( 1, "****** Map update time %f seconds\n", duration1);

    /// Get occupancy status
    int occupied_num=0;
    pcl::PointCloud<pcl::PointXYZ> cloud_to_publish;
    sensor_msgs::PointCloud2 cloud_to_pub_transformed;

    while(map_status_locked){
        loop_rate.sleep();
        ros::spinOnce();
    }

    map_status_locked = true;
    my_map.getOccupancyMapWithFutureStatus(occupied_num, cloud_to_publish, &future_status[0][0], 0.6);
    map_status_locked = false;


    /// Publish Point cloud and center position
    pcl::PointCloud<pcl::PointXYZ> cloud_to_publish_moved;
    for(auto &p : cloud_to_publish.points){
        pcl::PointXYZ p2;
        p2.x = p.x; // + map_center_point.x();
        p2.y = p.y; // + map_center_point.y();
        p2.z = p.z; // + map_center_point.z();
        cloud_to_publish_moved.push_back(p2);
    }

    pcl::toROSMsg(cloud_to_publish_moved, cloud_to_pub_transformed);
    cloud_to_pub_transformed.header.frame_id = "map";
    cloud_to_pub_transformed.header.stamp = ros::Time::now();
    cloud_pub.publish(cloud_to_pub_transformed);

    geometry_msgs::PoseStamped map_pose;
    map_pose.header.stamp = cloud_to_pub_transformed.header.stamp;
    map_pose.pose.position.x = current_position_sychronized.x();
    map_pose.pose.position.y = current_position_sychronized.y();
    map_pose.pose.position.z = current_position_sychronized.z();
    map_pose.pose.orientation.x = quad_rotate.x();
    map_pose.pose.orientation.y = quad_rotate.y();
    map_pose.pose.orientation.z = quad_rotate.z();
    map_pose.pose.orientation.w = quad_rotate.w();
    map_center_pub.publish(map_pose);

    /// Publish future status of one layer
    pcl::PointCloud<pcl::PointXYZRGB> future_status_cloud;
    static const int z_index_to_show = MAP_HEIGHT_VOXEL_NUM / 2 - 1; ///Layer
    for(int j=0; j<MAP_WIDTH_VOXEL_NUM; ++j){
        for(int i=0; i<MAP_LENGTH_VOXEL_NUM; ++i){
            int index_this = z_index_to_show*MAP_WIDTH_VOXEL_NUM*MAP_LENGTH_VOXEL_NUM + j*MAP_WIDTH_VOXEL_NUM + i;

            for(int n=0; n<PREDICTION_TIMES; ++n){
                pcl::PointXYZRGB p_this;
                float x_offset = (float)n * 12.f; //Used to show prediction at different times in one map

                my_map.getVoxelPositionFromIndexPublic(index_this, p_this.x, p_this.y, p_this.z);
                p_this.x += x_offset;

                float weight_this = future_status[index_this][n];
                int r, g, b;
                colorAssign(r, g, b, weight_this, 0.f, 0.1f, 1);
                p_this.r = r;
                p_this.g = g;
                p_this.b = b;
                future_status_cloud.push_back(p_this);
            }
        }
    }

    sensor_msgs::PointCloud2 cloud_future_transformed;
    pcl::toROSMsg(future_status_cloud, cloud_future_transformed);
    cloud_future_transformed.header.frame_id = "map";
    cloud_future_transformed.header.stamp = ros::Time::now();
    future_status_pub.publish(cloud_future_transformed);

//    finish2 = clock();
//    float duration2 = (float)(finish2 - start2) / CLOCKS_PER_SEC;
//    printf( "****** Map publish time %f seconds\n \n", duration2);

    last_time = this_time;

}





static void split(const string& s, vector<string>& tokens, const string& delimiters = " ")
{
    string::size_type lastPos = s.find_first_not_of(delimiters, 0);
    string::size_type pos = s.find_first_of(delimiters, lastPos);
    while (string::npos != pos || string::npos != lastPos) {
        tokens.push_back(s.substr(lastPos, pos - lastPos));
        lastPos = s.find_first_not_of(delimiters, pos);
        pos = s.find_first_of(delimiters, lastPos);
    }
}


void actorStatesCallback(const gazebo_msgs::ModelStates &msg){
    actor_states.clear();
    for(int i=0; i<msg.name.size(); ++i)
    {
        vector<string> name_splited;
        split(msg.name[i], name_splited, "_");
        if(name_splited[0] == "actor"){
            ActorState a;
            a.name = msg.name[i];
            a.record_time = ros::Time::now().toSec();
            a.px = msg.pose[i].position.x;
            a.py = msg.pose[i].position.y;
            a.pz = msg.pose[i].position.z;

            float w = msg.pose[i].orientation.w;
            float x = msg.pose[i].orientation.x;
            float y = msg.pose[i].orientation.y;
            float z = msg.pose[i].orientation.z;

            float bb = 2 * (w * z + x * y);
            float cc = 1 - 2 * (y * y + z * z);
            a.yaw = std::atan2(bb, cc);
            actor_states.push_back(a);

            // Calculate velocity
            bool found_pair = false;
            for(auto &item : actor_avg_states){
                if(a.name == item.name){
                    found_pair = true;

                    double delt_t = a.record_time - item.record_time;

                    if(delt_t < 0.05){  //update not faster than 20 Hz
                        break;
                    }

                    double vx_this = (a.px - item.px) / delt_t;
                    double vy_this = (a.py - item.py) / delt_t;
                    double vz_this = (a.pz - item.pz) / delt_t;

                    if(fabs(item.vx) < 0.00001f){
                        item.vx = vx_this;
                        item.vy = vy_this;
                        item.vz = vz_this;
                    }else{
                        item.vx = (vx_this + item.vx) / 2.f;
                        item.vy = (vy_this + item.vy) / 2.f;
                        item.vz = (vz_this + item.vz) / 2.f;
                    }

                    item.record_time = a.record_time;
                    item.px = a.px;
                    item.py = a.py;
                    item.pz = a.pz;
                    item.yaw = a.yaw;
                }
            }
            if(!found_pair){
                actor_avg_states.push_back(a);
            }

        }
    }

    /// Publish
    vector<Eigen::Vector3d> actor_visualization_points;
    for(auto const &item : actor_avg_states){
//       cout << item.name<<", v=(" << item.vx << ", "<< item.vy << ", " << item.vz <<") ";
        Eigen::Vector3d p;
        p.x() = item.px;
        p.y() = item.py;
        p.z() = item.pz;
        actor_visualization_points.push_back(p);
    }
//    cout << endl;

    marker_publish(actor_visualization_points, 6, 1.f, 0.f, 0.f, 0.4, -1);

}


void positionCallback(const geometry_msgs::PoseStamped& msg)
{
    if(!state_locked)
    {
        state_locked = true;

        /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
        // p0(0) = msg.pose.position.y;
        // p0(1) = -msg.pose.position.x;
        // p0(2) = msg.pose.position.z;

        // quad.x() = msg.pose.orientation.x;
        // quad.y() = msg.pose.orientation.y;
        // quad.z() = msg.pose.orientation.z;
        // quad.w() = msg.pose.orientation.w;

        // //Eigen::Quaternionf q1(0, 0, 0, 1);
        // Eigen::Quaternionf axis; //= quad * q1 * quad.inverse();
        // axis.w() = cos(-PI_2/2.0);
        // //axis.x() = axis.x() * sin(-PI_2/2.0);
        // //axis.y() = axis.y() * sin(-PI_2/2.0);
        // //axis.z() = axis.z() * sin(-PI_2/2.0);
        // axis.x() = 0.0;
        // axis.y() = 0.0;
        // axis.z() = sin(-PI_2/2.0);
        // quad = quad * axis;

        // NWU
        p0(0) = msg.pose.position.x;
        p0(1) = msg.pose.position.y;
        p0(2) = msg.pose.position.z;

        quad.x() = msg.pose.orientation.x;
        quad.y() = msg.pose.orientation.y;
        quad.z() = msg.pose.orientation.z;
        quad.w() = msg.pose.orientation.w;

        // Queue for synchronization
        pose_timestamp_queue.push(msg.header.stamp.toSec());
        pos_queue.push(p0);
        att_queue.push(quad);

        if(pose_timestamp_queue.size()>200){
            pose_timestamp_queue.pop();
            pos_queue.pop();
            att_queue.pop();
        }


        /// Update yaw0 here, should be among [-PI, PI]
        // yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));   /// TODO: CHECK if this is right!!!
        yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));


        state_locked = false;
    }

//    if(!offboard_ready){
//        init_p = p0;  //record a base point
//        p_goal(0) = p_goal_raw(0) + init_p(0);
//        p_goal(1) = p_goal_raw(1) + init_p(1);
//        p_goal(2) = init_p(2);
//        ROS_INFO_THROTTLE(1.0, "Waiting for offboard mode. p_goal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
//    }else{
//        ROS_INFO_THROTTLE(1.0, "IN offboard mode. pgoal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
//    }
//
//    ROS_INFO_THROTTLE(1.0, "Current pose. p0 = %f, %f, %f", p0(0), p0(1), p0(2));
}

void velocityCallback(const geometry_msgs::TwistStamped& msg)
{
    if(!state_locked)
    {
        state_locked = true;

        // /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
        // v0(0) = msg.twist.linear.y;
        // v0(1) = -msg.twist.linear.x;
        // v0(2) = msg.twist.linear.z;
        // yaw0_rate = msg.twist.angular.z;

        /**  NWU  **/
        v0(0) = msg.twist.linear.x;
        v0(1) = msg.twist.linear.y;
        v0(2) = msg.twist.linear.z;
        yaw0_rate = msg.twist.angular.z;

        if(fabs(v0(0)) < 0.05) v0(0) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(1)) < 0.05) v0(1) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(2)) < 0.05) v0(2) = 0.0;  //add a dead zone for v0 used in motion primatives

        float max_current_v = 0.8;
        for(int i=0; i<3; i++){
            if(v0(i) < -max_current_v){
                v0(i) = -max_current_v;
            }else if(v0(i) > max_current_v){
                v0(i) = max_current_v;
            }
        }

        /** Calculate virtual accelerates from velocity. Original accelerates given by px4 is too noisy **/
        static bool init_v_flag = true;
        static float last_time, last_vx, last_vy, last_vz;

        if(init_v_flag){
            init_v_flag = false;
        }
        else{
            double delt_t = ros::Time::now().toSec() - last_time;
            a0(0) = (v0(0) - last_vx) / delt_t;
            a0(1) = (v0(1) - last_vy) / delt_t;
            a0(2) = (v0(2) - last_vz) / delt_t;

            if(fabs(a0(0)) < 0.2) a0(0) = 0.0;  //dead zone for acc x
            if(fabs(a0(1)) < 0.2) a0(1) = 0.0; //dead zone for acc y
            if(fabs(a0(2)) < 0.2) a0(2) = 0.0; //dead zone for acc z

            float max_current_a = 1.0;
            for(int i=0; i<3; i++){
                if(a0(i) < -max_current_a){
                    a0(i) = -max_current_a;
                }else if(a0(i) > max_current_a){
                    a0(i) = max_current_a;
                }
            }

            //ROS_INFO("acc=(%f, %f, %f)", a0(0), a0(1), a0(2));
        }

        last_time = ros::Time::now().toSec();
        last_vx = v0(0);
        last_vy = v0(1);
        last_vz = v0(2);

        state_locked = false;
    }
}


void marker_publish(const Eigen::MatrixXd &Points, int id, float r, float g, float b, float width, int publish_num)
{
    if(Points.rows() < 1) return;

    visualization_msgs::Marker points;
    points.header.frame_id = "map";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";
    points.id = id;
    points.type = visualization_msgs::Marker::POINTS;

    // Line width
    points.scale.x = width;
    points.scale.y = width;

    points.color.r = r;
    points.color.g = g;
    points.color.b = b;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);

    int step = 1;
    if(publish_num > 0){
        step = (int)Points.rows() / publish_num;
    }

    for(int i=0; i<Points.rows(); i+=step)
    {
        geometry_msgs::Point p;
        p.x = Points(i, 0);
        p.y = Points(i, 1);
        p.z = Points(i, 2);

        points.points.push_back(p);
    }

    geometry_msgs::Point p;
    p.x = Points(Points.rows()-1, 0);
    p.y = Points(Points.rows()-1, 1);
    p.z = Points(Points.rows()-1, 2);
    points.points.push_back(p);

    current_marker_pub.publish(points);
}


void marker_publish(const vector<Eigen::Vector3d> &Points, int id, float r, float g, float b, float width, int publish_num)
{
    if(Points.empty()) return;

    visualization_msgs::Marker points;
    points.header.frame_id = "map";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";
    points.id = id;
    points.type = visualization_msgs::Marker::POINTS;

    // Line width
    points.scale.x = width;
    points.scale.y = width;

    points.color.r = r;
    points.color.g = g;
    points.color.b = b;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);

    int step = 1;
    if(publish_num > 0){
        step = Points.size() / publish_num;
    }
    step = max(1, step);

    for(int i=0; i<Points.size(); i+=step)
    {
        geometry_msgs::Point p;
        p.x = Points[i](0) - map_center_point.x();
        p.y = Points[i](1) - map_center_point.y();
        p.z = Points[i](2) - map_center_point.z();
        points.points.push_back(p);
    }

    geometry_msgs::Point p;
    p.x = Points[Points.size()-1](0) - map_center_point.x();
    p.y = Points[Points.size()-1](1) - map_center_point.y();
    p.z = Points[Points.size()-1](2) - map_center_point.z();
    points.points.push_back(p);

    current_marker_pub.publish(points);
}



void getParameterList(const ros::NodeHandle& nh) {
    nh.getParam("/planning_test/p_goal_x", p_goal(0));
    nh.getParam("/planning_test/p_goal_y", p_goal(1));
    nh.getParam("/planning_test/p_goal_z", p_goal(2));
    nh.getParam("/planning_test/MAX_V_XY", MAX_V_XY);
    nh.getParam("/planning_test/MAX_V_Z_UP", MAX_V_Z_UP);
    nh.getParam("/planning_test/MAX_V_Z_DOWN", MAX_V_Z_DOWN);
    nh.getParam("/planning_test/MAX_A", MAX_A);

    nh.getParam("/planning_test/trajectory_1_send_counter", trajectory_1_send_counter);
    nh.getParam("/planning_test/ABSOLUTE_SAFE_TIME_PIECE1", ABSOLUTE_SAFE_TIME_PIECE1);
    nh.getParam("/planning_test/ABSOLUTE_SAFE_TIME_PIECE2", ABSOLUTE_SAFE_TIME_PIECE2);

    nh.getParam("/planning_test/d_ref", pp.d_ref);
    nh.getParam("/planning_test/k1_xy", pp.k1_xy);
    nh.getParam("/planning_test/k1_z", pp.k1_z);
    nh.getParam("/planning_test/k2_xy", pp.k2_xy);
    nh.getParam("/planning_test/k2_z", pp.k2_z);
    nh.getParam("/planning_test/v_max_at_goal_ori", pp.v_max_at_goal_ori);
    nh.getParam("/planning_test/collision_threshold_static", pp.collision_threshold_static);
    nh.getParam("/planning_test/risk_checking_radius", pp.risk_checking_radius);
    nh.getParam("/planning_test/dynamic_future_risk_threshold", pp.dynamic_future_risk_threshold);

    nh.getParam("/planning_test/if_plan_vertical_path", if_plan_vertical_path);
    nh.getParam("/planning_test/if_in_simulation", if_in_simulation);
    nh.getParam("/planning_test/if_flyback_enable", if_flyback_enable);
    nh.getParam("/planning_test/HEIGHT_LIMIT", HEIGHT_LIMIT);
    nh.getParam("/planning_test/XY_LIMIT", XY_LIMIT);
    nh.getParam("/planning_test/PLAN_INIT_STATE_CHANGE_THRESHOLD", PLAN_INIT_STATE_CHANGE_THRESHOLD);
    nh.getParam("/planning_test/DIRECTION_CHANGE_LEFT_SIZE", DIRECTION_CHANGE_LEFT_SIZE);
    nh.getParam("/planning_test/DIRECTION_AUTO_CHANGE", DIRECTION_AUTO_CHANGE);
    nh.getParam("/planning_test/MAP_DELAY_SECONDS", MAP_DELAY_SECONDS);
}


int main(int argc, char** argv) {

    ros::init(argc, argv, "planning_test");
    ros::NodeHandle nh;

    setParameters();
    getParameterList(nh); /// chg, use parameters defined in the yaml file
    ROS_WARN("MAX_V=%f, MAX_A=%f, V_final_ref=%f", MAX_V_XY, MAX_A, pp.v_max_at_goal_ori);

    my_map.setPredictionVariance(0.05, 0.1);  //0.1, 0.1
    my_map.setObservationStdDev(0.01);
//    my_map.setParticleRecordFlag(1, 22.5);
    my_map.setNewBornParticleNumberofEachPoint(30);
    my_map.setNewBornParticleWeight(0.01);
    VideMap::setOriginalVoxelFilterResolution(res);


    trajectory_time_table->csv2pva_table("/home/clarence/ros_ws/map_ws/src/tables/p3-2_v3_a4_res0-1.csv");
    ROS_WARN("trajectory_time_table loaded!");

    p0 << 0.0, 0.0, 0.0;
    v0 << 0.0, 0.0, 0.0;
    a0 << 0.0, 0.0, 0.0;
//    p_goal << -1.0, -5.0, 1.2;
    yaw0 = 0.0;

    // Fov sample parameters
    Fov_half << 35, 20;
    // Horizontal angles larger than 90 degree are deleted
    //Angle_h_sparse << -90, -70, -50, -30, -20, -10, 0, 10, 20, 30, 50, 70, 90;
    if(if_flyback_enable){
//        Angle_h_sparse << -130, -90, -75, -60, -45, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 45, 60, 75, 90, 130;
        Angle_h_sparse << -120, -100, -80, -70, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 70, 80, 100, 120;
    }else{
//        Angle_h_sparse << -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 5, 10, 20, 30, 40, 50, 60, 70, 80, 90;
        Angle_h_sparse << -88, -80, -72, -64, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 64, 72, 80, 88;

    }

    if(if_plan_vertical_path){
        // Angle_v_sparse << -60, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 60;
        Angle_v_sparse << -24, -16, -8, 0, 8, 16, 24;
    } else{
        Angle_v_sparse << 0, 0, 0, 0, 0, 0, 0;
    }

    if(Angle_h_sparse.size() != ANGLE_H_NUM_SPARSE || Angle_v_sparse.size() != ANGLE_V_NUM_SPARSE){
        ROS_INFO(" ANGLE_H_NUM_SPARSE or ANGLE_V_NUM_SPARSE mismatches the initialized vector. Please check!");
        return 0;
    }

    Angle_h_sparse = Angle_h_sparse * M_PI / 180.0; // degree to rad
    Angle_v_sparse = Angle_v_sparse * M_PI / 180.0;
    Fov_half = Fov_half * M_PI / 180.0;

    for(auto & item : future_status){
        for(float & j : item){
            j = 0.f;
        }
    }

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1, true);
    current_marker_pub = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 1);

    future_status_pub = nh.advertise<sensor_msgs::PointCloud2>("/my_map/future_status", 1, true);
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/my_map/cloud_ob", 1, true);
    map_center_pub = nh.advertise<geometry_msgs::PoseStamped>("/map_center",1,true) ;

    ros::Subscriber pedestrian_sub = nh.subscribe("/gazebo/model_states", 1, actorStatesCallback);
    ros::Subscriber position_isolate_sub =  nh.subscribe("/mavros/local_position/pose", 1, positionCallback);
    ros::Subscriber velocity_isolate_sub = nh.subscribe("/mavros/local_position/velocity_local", 1, velocityCallback);

    ros::Subscriber cloud_sub = nh.subscribe("/camera_front/depth/points", 1, cloudCallback);

    ros::Timer timer1 = nh.createTimer(ros::Duration(0.1), trajectoryCallback);

    ros::Timer timer2 = nh.createTimer(ros::Duration(CAL_DURATION), sendCallback);


//    ros::spin();
    ros::AsyncSpinner spinner(3); // Use 3 threads
    spinner.start();
    ros::waitForShutdown();

    return 0;
}


void setParameters(){
    p_goal(0) = 60.0;
    p_goal(1)= 0.0;
    p_goal(2)=1.5;
    p_goal_raw = p_goal;

    MAX_V_XY = 3;
    MAX_V_Z_UP = 0.8;
    MAX_V_Z_DOWN = 0.4;
    MAX_A = 4;
    trajectory_1_send_counter = 1;  // To eliminate the influence of delay
    ABSOLUTE_SAFE_TIME_PIECE1 = 1.f;
    ABSOLUTE_SAFE_TIME_PIECE2 = 1.f;
    pp.d_ref = 2.2;
    pp.k1_xy = 8.0;
    pp.k1_z = 8.0;
    pp.k2_xy = 4.0;
    pp.k2_z = 4.0;
    pp.v_max_at_goal_ori = 0.5;
    pp.collision_threshold_static = 0.2;
    pp.risk_checking_radius = 0.5;
    pp.dynamic_future_risk_threshold = 40.0;

    if_plan_vertical_path = true;
    if_in_simulation = true;
    if_flyback_enable = true;
    HEIGHT_LIMIT = 2.5;
    XY_LIMIT = 2.2;
    PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;
    DIRECTION_CHANGE_LEFT_SIZE = 1.5;
    DIRECTION_AUTO_CHANGE = false;
    MAP_DELAY_SECONDS = 0.05;
}